// RungeKutta45 -- a 4th Order Runge-Kutta Solver with adaptivity
//                 (uses Runge-Kutta-Fehlberg)
// Robert McDougal, 4 April 2007

import java.util.Vector;

public class RungeKutta45 extends DifferentialEquationSolver {
    private double length = 1;
    public double h = .001;
    public double maxH = .01;
    private boolean shownLossOfAccuracy = false ;
    
    // the absolute error (in 2-norm) per each advanceTo should be less than eps_a
    public double EPS_A = .00001;
    
    public double MIN_STEP = .00001;
    // pass constructor arguments to general case
    public RungeKutta45(IOdeRhs yPrime, double[] y0, double t0) {
        super(yPrime, y0, t0);
    }
    
    
   /**
     * Integrates the ODE using Runge-Kutta-Fehlberg until time t with suggested
     * step size h.
     *
     * @param t time to integrate to
     * @param h suggested step size
     * @return vector of variables after advance
     */
    public double[] advanceTo(double t, double h) {
        shownLossOfAccuracy = false ;
        length = t - t0;
        this.h = h;
        while (t0 < t) {
            if (this.h+t0 < t) {
                step(this.h);
                // impose an upper bound on step size
                if (this.h>maxH) {
                    this.h = maxH ;
                }
            } else {
                step(t - t0);
                t0 = t;
            }
        }
        return y0.clone();
    }
    
   /**
     * Performs a step towards the solution of the ODE with 4th order RK.
     *
     * @param h step size to use
     * @return vector of variables after advance
     */
    public double[] step(double h) {
        return step(h,4);
    }
    
   /**
     * Performs a step towards the solution of the ODE with nth order RK.
     *
     * @param h step size to use
     * @param n order of the RK method to use.  only 4 and 5 are implemented.
     * @return vector of variables after advance
     */
    public double[] step(double h, int n) {
        double[] s1 = yPrime.eval(t0, y0);
        double[] s2 = yPrime.eval(t0+h/4, add(y0,multiply(.25*h,s1)));
        double[] s3 = yPrime.eval(t0+.375*h, add(y0,multiply(h,add(multiply(3./32,s1),multiply(9./32,s2)))));
        double[] s4 = yPrime.eval(t0+12./13*h, add(y0,multiply(h,add(multiply(1932./2197,s1),
                add(multiply(-7200./2197,s2),multiply(7296./2197,s3))))));
        double[] s5 = yPrime.eval(t0+h, add(y0,multiply(h,add(add(multiply(439./216,s1),multiply(-8,s2)),
                add(multiply(3680./513,s3),multiply(-845./4104,s4))))));
        double[] s6 = yPrime.eval(t0+h/2, add(y0,multiply(h,add(add(multiply(-8./27,s1),multiply(2,s2)),
                add(multiply(-3544./2565,s3),add(multiply(1859./4104,s4),multiply(-11./40,s5)))))));
        double[] y4 = add(y0,multiply(h,add(add(multiply(25./216,s1),multiply(1408./2565,s3)),
                add(multiply(2197./4104,s4),multiply(-.2,s5)))));
        double[] y5 = add(y0,multiply(h,add(add(multiply(16./135,s1),multiply(6656./12825,s3)),
                add(multiply(28561./56430,s4),add(multiply(-9./50,s5),multiply(2./55,s6))))));
        double temp ;
        if ( n != 4 && n != 5 ) {
            return null;
        } else if ( n == 4 ) {
            y0 = y4;
        } else {
            y0 = y5;
        }
        temp = .8 * h * Math.pow(h*EPS_A/(length*norm(subtract(y5, y4))),.25);
        if ( ! ( Double.isNaN ( temp ) || Double.isInfinite ( temp ) ) ) {
            this.h = temp ;
        }
        if (this.h< MIN_STEP) {
            if (!shownLossOfAccuracy) {
                System.out.println("Possible loss of accuracy.");
                shownLossOfAccuracy = true ;
            }
            this.h = MIN_STEP;
        }
        if ( !isFinite( y0 ) ) {
            this.h = Double.POSITIVE_INFINITY;
        }
        t0 += h;
        return y0.clone();
    }
    
    // y'=y
    private static class DiffEq1 implements IOdeRhs {
        public double[] eval( double t , double[] y ) {
            return y ;
        }
    }
    
    public static void main( String[] args ) {
        double[] y = new double [ 1 ] ;
        double[] z = null;
        int order;
        IOdeRhs diffEq = new DiffEq1( ) ;
        RungeKutta45 solver ;
        int n, i;
        double h;
        y[0]=1;
        for ( order=4; order <= 5; order++ ) {
            System.out.println("** ORDER "+order+" **");
            for ( n = 16; n <= 65536; n *= 2 ) {
                h = 1. / n ;
                solver = new RungeKutta45(diffEq, y, 0);
                for ( i=0; i<n; i++ ) {
                    z = solver.step(h, order);
                }
                System.out.println(""+n+'\t'+Math.abs(z[0]-Math.E));
            }
            System.out.println("\n\n");
        }
        
        System.out.println("\n\n");
        System.out.println("** TESTING ADAPTIVE ROUTINE **" );
        System.out.println("Requested, Received Errors:");
        for ( double err = .00001; err >= .00000000001; err *= .1 ) {
            solver = new RungeKutta45(diffEq, y, 0);
            solver.EPS_A = err;
            z = solver.advanceTo(1,.01);
            System.out.println("" + err + '\t' + Math.abs(z[0]-Math.E));
        }
    }
    
}

